﻿using System;
using System.Collections;
using System.IO;
using System.Xml;
using CommonNamespace;
using MatrixDataStructures;
using MetaView;
using twoDMatrixLibrary;

namespace DWICalculator
{
    class Program
    {
        static MatrixDataStructures.MultiVolume dataMatrix;
        static int NumberofGradients = 0;
        static twoDMatrixLibrary.Matrix LoadedDTIDirections;

        static Random randomGen;
        static double stDev;
        static int iterationsCounter =0;

        static void Main(string[] args)
        {
            if (args.Length == 0)
            {
                Console.Write("Enter filename with path: ");
                args = new string[] { Console.ReadLine() };
            }
            
            if (!System.IO.File.Exists(args[0].ToString()))
            {
                Console.WriteLine("File not found. Aborted.");
                return;
            }

            Console.WriteLine();
            Console.Write("Enter no. of Iterations: ");
            iterationsCounter = int.Parse(Console.ReadLine());
            Console.WriteLine();
            if (iterationsCounter > 0)
            {
                Console.Write("Enter Standard Deviation for Gaussian Function: ");
                stDev = double.Parse(Console.ReadLine());
                Console.WriteLine();
            }
            
            randomGen = new Random();

            Console.WriteLine("File found. Loading DATA to memory ... ");
            DateTime startTime = DateTime.Now;
            FileInfo fname = new FileInfo(args[0]);
        
            if (fname.Extension == ".HDR" | fname.Extension == ".hdr")
            {
                dataMatrix = new MultiVolume();
                dataMatrix.Matrix = null;
                string[] files = System.IO.Directory.GetFiles(System.IO.Path.GetDirectoryName(args[0].ToString()));
                System.Collections.SortedList validNameList = new System.Collections.SortedList();
                foreach (string file in files)
                {
                    int posindex = System.IO.Path.GetFileNameWithoutExtension(file).LastIndexOf("_");
                    if (posindex <= 1)
                        continue;
                    if (file.Substring(0, posindex - 1) == args[0].ToString().Substring(0, posindex - 1) & System.IO.Path.GetExtension(file).ToLower() == ".hdr")
                    {
                        string filenumber = System.IO.Path.GetFileNameWithoutExtension(file);
                        filenumber = filenumber.Substring(posindex, filenumber.Length - (posindex));
                        filenumber = filenumber.Trim('_');
                        validNameList.Add(Int32.Parse(filenumber), file);
                    }
                }
                int loadcounter = 0;
                IDictionaryEnumerator ide = validNameList.GetEnumerator();
                if (validNameList.Keys.Count < 5)
                {
                    Console.WriteLine("File format should be filename_0xy.hdr where xy=00-99");
                    return;
                }
                while ((ide.MoveNext()))
                {
                    SingleVolume tempv = CommonNamespace.HdrImgReader.ReadFromHdrFile(ide.Value.ToString(), HdrImgReader.OutPutType.int16);
                    if (dataMatrix.Matrix == null)
                    {
                        dataMatrix.Matrix = new Matrix4DInt16(tempv.Matrix.x, tempv.Matrix.y, tempv.Matrix.z, validNameList.Count);
                        Console.WriteLine("Diffusion Matrix initialized to : " + tempv.Matrix.x + "," + tempv.Matrix.y + "," + tempv.Matrix.z + "," + validNameList.Count);
                    }
                    dataMatrix.Matrix.LoadVolume(loadcounter, tempv.Matrix.data);
                    loadcounter += 1;
                }
            }
            else if (fname.Extension == ".par" | fname.Extension == ".PAR")
            {
                MultipleOptimizedParRecLoader Loader = new MultipleOptimizedParRecLoader(args[0].ToString());
                dataMatrix = Loader.GetData();
                dataMatrix.HeaderFileName = args[0].ToString();
            }

            Console.WriteLine("DATA Loaded in " + (DateTime.Now - startTime).TotalSeconds + " seconds.");
            
            Console.WriteLine("Reading Scanner Parameters ...");
            startTime = DateTime.Now;
            float smallD, bigD, gradStrength;
            System.Collections.ArrayList directionsList;
            ParseScannerParamFile(System.IO.Path.GetDirectoryName(args[0]) + System.IO.Path.DirectorySeparatorChar + "DTI Parameters.xml", out bigD, out smallD, out gradStrength, out directionsList);            
            LoadedDTIDirections = new twoDMatrixLibrary.Matrix(directionsList.Count, 3);
            NumberofGradients = directionsList.Count;
            double b = (2.675e4 * 2.675e4) * (gradStrength * gradStrength) * (bigD * bigD) * (bigD - smallD / 3);
            Console.WriteLine("           Calculated B-value: " + b);
            Console.Write(    "           Do you want to change b-value (y,n) ? ");
            if (Console.ReadKey().Key == ConsoleKey.Y)
            {
                Console.WriteLine();
                Console.WriteLine("           Enter b-value: ");
                b = double.Parse(Console.ReadLine().Trim());
            }
            else
                Console.WriteLine();
            for (int i = 0; i < directionsList.Count; i++)
            {
                LoadedDTIDirections[i, 0] = ((float[])directionsList[i])[0];
                LoadedDTIDirections[i, 1] = ((float[])directionsList[i])[1];
                LoadedDTIDirections[i, 2] = ((float[])directionsList[i])[2];
            }
            Console.WriteLine("Completed in " + (DateTime.Now - startTime).TotalSeconds + " seconds.");

            if (iterationsCounter == 0)
            {
                Go(false, 0, b,true);
                return;
            }
                else
                Go(false,0,b,false);            
            for (int counter=0; counter< iterationsCounter; counter++)
            {
                Console.WriteLine();
                Console.WriteLine();
                Go(true, counter +1,b,false); //mark 0 as the one without noise
                Console.WriteLine();
             
            }
            Console.WriteLine();
            Console.WriteLine();
            
        }

        static void Go( bool addNoise, int counter, double b, bool maps)
        {
            DateTime startTime = DateTime.Now;

            if (addNoise)
            {
                Console.WriteLine("Adding Gaussian Noise ...");
                startTime = DateTime.Now;
                for (int i = 0; i < dataMatrix.Matrix.i; i++)
                    for (int j = 0; j < dataMatrix.Matrix.j; j++)
                        for (int k = 0; k < dataMatrix.Matrix.k; k++)
                            for (int l = 0; l < dataMatrix.Matrix.l; l++)
                                dataMatrix.Matrix.data[i, j, k, l] += Convert.ToInt16((1 / ((Math.Pow((2 * Math.PI), 0.5)) * stDev)) * Math.Exp(-1 * ((randomGen.Next() - dataMatrix.Matrix.data[i, j, k, l]) ^ 2) / (2 * (Math.Pow(stDev,2)))));             
                Console.WriteLine("Completed in " + (DateTime.Now - startTime).TotalSeconds + " seconds.");
            }

            startTime = DateTime.Now;            
            Console.WriteLine("Creating Ratio Matrices ...");
            startTime = DateTime.Now;
            StartDTIAnalysis();
            Console.WriteLine("Completed in " + (DateTime.Now - startTime).TotalSeconds + " seconds.");

            Console.WriteLine("Computing B-Matrix and Diffusion Matrix ...");
            startTime = DateTime.Now;
            ComputeDiffusionMatrix(b);
            Console.WriteLine("Completed in " + (DateTime.Now - startTime).TotalSeconds + " seconds.");

            Console.WriteLine("Performing Eigenvalue Decomposition ...");
            startTime = DateTime.Now;
            DiagonalizeComputedDiffusionMatrix();
            Console.WriteLine("Completed in " + (DateTime.Now - startTime).TotalSeconds + " seconds.");

            //Console.WriteLine("Current RAM Usage: " + getAvailableRAM() + " | CPU: " + getCurrentCpuUsage());

            Console.WriteLine("Saving Eigenvalue Maps ...");
            startTime = DateTime.Now;
            HdrImgWriter.WriteToFile(new SingleVolume(null, EigenValue1, dataMatrix.FOV, dataMatrix.Center, dataMatrix.OffCenterAngle), "c:\\output_1_" + counter, HdrImgWriter.DataType.DT_FLOAT);
            HdrImgWriter.WriteToFile(new SingleVolume(null, EigenValue2, dataMatrix.FOV, dataMatrix.Center, dataMatrix.OffCenterAngle), "c:\\output_2_" + counter, HdrImgWriter.DataType.DT_FLOAT);
            HdrImgWriter.WriteToFile(new SingleVolume(null, EigenValue3, dataMatrix.FOV, dataMatrix.Center, dataMatrix.OffCenterAngle), "c:\\output_3_" + counter, HdrImgWriter.DataType.DT_FLOAT);
            Console.WriteLine("Completed in " + (DateTime.Now - startTime).TotalSeconds + " seconds.");

            if (maps)
            {
                Matrix3DSingle faData = new Matrix3DSingle(EigenValue1.x, EigenValue1.y, EigenValue1.z);
                for (int i = 0; i < EigenValue1.x; i++)
                    for (int j = 0; j < EigenValue1.y; j++)
                        for (int k = 0; k < EigenValue1.z; k++)
                        {
                            double denominator = EigenValue1[i, j, k] * EigenValue1[i, j, k] + EigenValue2[i, j, k] * EigenValue2[i, j, k] + EigenValue3[i, j, k] * EigenValue3[i, j, k];
                            if (denominator != 0)
                                faData[i, j, k] = (float)(0.7071 * Math.Pow(
                                                                     Math.Pow(EigenValue1[i, j, k] - EigenValue2[i, j, k], 2)
                                                                   + Math.Pow(EigenValue2[i, j, k] - EigenValue3[i, j, k], 2)
                                                                   + Math.Pow(EigenValue3[i, j, k] - EigenValue1[i, j, k], 2)
                                                                   , 0.5)*
                                                            1/denominator);
                            if (faData[i, j, k] > 1)
                                faData[i, j, k] = 1;
                        }

                HdrImgWriter.WriteToFile(new SingleVolume(null, faData, dataMatrix.FOV, dataMatrix.Center, dataMatrix.OffCenterAngle), "c:\\fa", HdrImgWriter.DataType.DT_FLOAT);            
            }
            
        }

        # region "DTI Parameter file parser"
        static void ParseScannerParamFile(string filename, out float bigDelta, out float smallDelta, out float gradStrength, out System.Collections.ArrayList directionsArray)
        {
            XmlDocument xmlDoc;
            xmlDoc = new XmlDocument();
            xmlDoc.Load(filename);

            XmlNode grad_str, big_d, small_d, dir_count;
            bigDelta = new float();
            smallDelta = new float();
            gradStrength = new float();
            directionsArray = new System.Collections.ArrayList();

            if (xmlDoc.DocumentElement.HasChildNodes)
            {

                //first
                grad_str = xmlDoc.DocumentElement.FirstChild;
                if (grad_str.Name == "gradient_strength")
                {
                    gradStrength = float.Parse(grad_str.ChildNodes[0].Value);
                }

                big_d = xmlDoc.ChildNodes[1].ChildNodes[1];
                if (big_d.Name == "big_d")
                {
                    bigDelta = float.Parse(big_d.ChildNodes[0].Value);
                }
                //next
                small_d = xmlDoc.ChildNodes[1].ChildNodes[2];
                if (small_d.Name == "small_d")
                {
                    smallDelta = float.Parse(small_d.ChildNodes[0].Value);
                }

                dir_count = xmlDoc.ChildNodes[1].ChildNodes[3];
                int dir_c = (int)double.Parse(dir_count.ChildNodes[0].Value) * 3;

                int i;
                for (i = 1; i <= dir_c; i += 3)
                {
                    float[] dir = new float[3];
                    XmlNode dir_node;
                    dir_node = xmlDoc.ChildNodes[1].ChildNodes[3 + i];
                    string direction = dir_node.ChildNodes[0].Value;
                    dir[0] = float.Parse(direction);
                    dir_node = xmlDoc.ChildNodes[1].ChildNodes[i + 4];
                    direction = dir_node.ChildNodes[0].Value;
                    dir[1] = float.Parse(direction);
                    dir_node = xmlDoc.ChildNodes[1].ChildNodes[i + 5];
                    direction = dir_node.ChildNodes[0].Value;
                    dir[2] = float.Parse(direction);
                    directionsArray.Add(dir);
                }
            }
        }
        # endregion

        # region "Subtracted File creator"
        static Matrix4Ddouble SubtractedMatrix;
        static void StartDTIAnalysis()
        {
            int i;
            Matrix4Ddouble logmatrix = new Matrix4Ddouble(dataMatrix.Matrix.i, dataMatrix.Matrix.j, dataMatrix.Matrix.k, dataMatrix.Matrix.l);
            //Replace all zeros by 1
            logmatrix.Replace(0, 1);
            logmatrix.data = dataMatrix.Matrix.Log(false);
            SubtractedMatrix = new Matrix4Ddouble(dataMatrix.Matrix.i, dataMatrix.Matrix.j, dataMatrix.Matrix.k, dataMatrix.Matrix.l);
            for (i = 1; i <= NumberofGradients; i++)
            {
                SubtractedMatrix = load_slices_into_volume(subtract_volumes(load_slices_from_volume(logmatrix, 0), load_slices_from_volume(logmatrix, i)), SubtractedMatrix, i - 1);
            }

        }

        static Matrix4Ddouble load_slices_into_volume(Matrix3DSingle slices_matrix, Matrix4Ddouble volumes_matrix, int volume_no)
        {
            int a;
            int b;
            int c;
            for (a = 0; a <= slices_matrix.x - 1; a++)
            {
                for (b = 0; b <= slices_matrix.y - 1; b++)
                {
                    for (c = 0; c <= slices_matrix.z - 1; c++)
                    {
                        volumes_matrix.data[a, b, c, volume_no] = slices_matrix.data[a, b, c];
                    }
                }
            }
            return volumes_matrix;
        }
        static Matrix3DSingle load_slices_from_volume(Matrix4Ddouble data, int volume_no)
        {
            Matrix3DSingle ans = new Matrix3DSingle(data.i, data.j, data.k);
            int i;
            int j;
            int k;
            for (i = 0; i <= data.i - 1; i++)
            {
                for (j = 0; j <= data.j - 1; j++)
                {
                    for (k = 0; k <= data.k - 1; k++)
                    {
                        ans.data[i, j, k] = (float)data.data[i, j, k, volume_no];
                    }
                }
            }
            return ans;
        }
        static MatrixDataStructures.Matrix3DSingle subtract_volumes(MatrixDataStructures.Matrix3DSingle Matrix1, MatrixDataStructures.Matrix3DSingle Matrixn)
        {
            MatrixDataStructures.Matrix3DSingle ans = new MatrixDataStructures.Matrix3DSingle(Matrix1.x, Matrix1.y, Matrix1.z);
            //do twoDMatrixLibrary.Matrix 1 - twoDMatrixLibrary.Matrix n
            int i;
            int j;
            int k;
            for (i = 0; i <= Matrix1.x - 1; i++)
            {
                for (j = 0; j <= Matrix1.y - 1; j++)
                {
                    for (k = 0; k <= Matrix1.z - 1; k++)
                    {
                        ans.data[i, j, k] = Matrix1.data[i, j, k] - Matrixn.data[i, j, k];
                    }
                }
            }
            return ans;
        }

        #endregion

        # region "B-Matrix and Diffusivity Matrix creator"        
        static Matrix4Ddouble DiffusionMatrix;
        static void ComputeDiffusionMatrix(double b)
        {
            twoDMatrixLibrary.Matrix x = new twoDMatrixLibrary.Matrix(NumberofGradients, 6);
            int i;
            int Nx = dataMatrix.Matrix.i;
            int Ny = dataMatrix.Matrix.j;
            int Nz = dataMatrix.Matrix.k;
            for (i = 0; i <= NumberofGradients - 1; i++)
            {
                x[i, 0] = LoadedDTIDirections[i, 0] * LoadedDTIDirections[i, 0] * b;
                x[i, 1] = LoadedDTIDirections[i, 1] * LoadedDTIDirections[i, 1] * b;
                x[i, 2] = LoadedDTIDirections[i, 2] * LoadedDTIDirections[i, 2] * b;
                x[i, 3] = 2 * LoadedDTIDirections[i, 0] * LoadedDTIDirections[i, 1] *b;
                x[i, 4] = 2 * LoadedDTIDirections[i, 1] * LoadedDTIDirections[i, 2] * b;
                x[i, 5] = 2 * LoadedDTIDirections[i, 0] * LoadedDTIDirections[i, 2]*b;
            }
            twoDMatrixLibrary.Matrix ls = ((x.Transpose() * x).Inverse) * x.Transpose();
            DiffusionMatrix = new Matrix4Ddouble(Nx, Ny, Nz, 6);
            Matrix3DSingle res = new Matrix3DSingle(Nx, Ny, Nz);
            Matrix3DSingle trd = new Matrix3DSingle(Nx, Ny, Nz);
            int j;
            int k;
            for (i = 0; i <= Nx - 1; i++)
            {
                for (j = 0; j <= Ny - 1; j++)
                {
                    for (k = 0; k <= Nz - 1; k++)
                    {
                        twoDMatrixLibrary.Matrix temp = new twoDMatrixLibrary.Matrix(NumberofGradients, 1);

                        int n;
                        for (n = 0; n <= NumberofGradients - 1; n++)
                        {
                            temp[n, 0] = SubtractedMatrix.data[i, j, k, n];
                        }

                        twoDMatrixLibrary.Matrix dtemp = (ls * temp);
                        twoDMatrixLibrary.Matrix dif = temp - (x * dtemp);

                        res.data[i, j, k] = (float)((Math.Sqrt((dif.Transpose() * dif)[0, 0])) / (Math.Pow((NumberofGradients - 5), 0.5)));
                        trd.data[i, j, k] = (float)(dtemp[0, 0] + dtemp[1, 0] + dtemp[2, 0]);

                        if (trd.data[i, j, k] / res.data[i, j, k] > 0)
                        {
                            //check for threshold
                            DiffusionMatrix.data[i, j, k, 0] = dtemp[0, 0];
                            DiffusionMatrix.data[i, j, k, 1] = dtemp[1, 0];
                            DiffusionMatrix.data[i, j, k, 2] = dtemp[2, 0];
                            DiffusionMatrix.data[i, j, k, 3] = dtemp[3, 0];
                            DiffusionMatrix.data[i, j, k, 4] = dtemp[4, 0];
                            DiffusionMatrix.data[i, j, k, 5] = dtemp[5, 0];
                        }
                    }
                }
            }
        }
        #endregion

        # region "Eigen-Decomposition"
        static  Matrix4Ddouble EigenVector1, EigenVector2, EigenVector3;
        static Matrix3DSingle EigenValue1, EigenValue2, EigenValue3;
        static void DiagonalizeComputedDiffusionMatrix()
        {
                        int Nx = dataMatrix.Matrix.i;
            int Ny = dataMatrix.Matrix.j;
            int Nz = dataMatrix.Matrix.k;
            EigenValue1 = new Matrix3DSingle(Nx, Ny, Nz);
            EigenValue2 = new Matrix3DSingle(Nx, Ny, Nz);
            EigenValue3 = new Matrix3DSingle(Nx, Ny, Nz);
            EigenVector1 = new Matrix4Ddouble(Nx, Ny, Nz, 3);
            EigenVector2 = new Matrix4Ddouble(Nx, Ny, Nz, 3);
            EigenVector3 = new Matrix4Ddouble(Nx, Ny, Nz, 3);
            double Max = 0;
            int a;
            int b;
            int c;

            for (a = 0; a <= Nx - 1; a++)
            {
                for (b = 0; b <= Ny - 1; b++)
                {
                    for (c = 0; c <= Nz - 1; c++)
                    {
                        twoDMatrixLibrary.Matrix dtemp = new twoDMatrixLibrary.Matrix(3, 3, 0);
                        dtemp[0, 0] = DiffusionMatrix.data[a, b, c, 0];
                        dtemp[0, 1] = DiffusionMatrix.data[a, b, c, 3];
                        dtemp[0, 2] = DiffusionMatrix.data[a, b, c, 5];
                        dtemp[1, 0] = DiffusionMatrix.data[a, b, c, 3];
                        dtemp[1, 1] = DiffusionMatrix.data[a, b, c, 1];
                        dtemp[1, 2] = DiffusionMatrix.data[a, b, c, 4];
                        dtemp[2, 0] = DiffusionMatrix.data[a, b, c, 5];
                        dtemp[2, 1] = DiffusionMatrix.data[a, b, c, 4];
                        dtemp[2, 2] = DiffusionMatrix.data[a, b, c, 2];
                        EigenvalueDecomposition dtemp2 = new EigenvalueDecomposition(dtemp);
                        EigenValue1.data[a, b, c] = (float)dtemp2.RealEigenvalues[0];
                        EigenValue2.data[a, b, c] = (float)dtemp2.RealEigenvalues[1];
                        EigenValue3.data[a, b, c] = (float)dtemp2.RealEigenvalues[2];
                        if (EigenValue1[a, b, c] > Max)
                            Max = EigenValue1[a, b, c];
                        EigenVector1.data[a, b, c, 0] = dtemp2.EigenvectorMatrix[0, 0];
                        EigenVector1.data[a, b, c, 1] = dtemp2.EigenvectorMatrix[1, 0];
                        EigenVector1.data[a, b, c, 2] = dtemp2.EigenvectorMatrix[2, 0];
                        EigenVector2.data[a, b, c, 0] = dtemp2.EigenvectorMatrix[0, 1];
                        EigenVector2.data[a, b, c, 1] = dtemp2.EigenvectorMatrix[1, 1];
                        EigenVector2.data[a, b, c, 2] = dtemp2.EigenvectorMatrix[2, 1];
                        EigenVector3.data[a, b, c, 0] = dtemp2.EigenvectorMatrix[0, 2];
                        EigenVector3.data[a, b, c, 1] = dtemp2.EigenvectorMatrix[1, 2];
                        EigenVector3.data[a, b, c, 2] = dtemp2.EigenvectorMatrix[2, 2];

                    }
                }
            }


        }
        #endregion 

        //# region "FA calculation"
        //static Matrix3DSingle FAMatrix;
        //static void CalculateFA()
        //{
        //    int Nx = dataMatrix.Matrix.i;
        //    int Ny = dataMatrix.Matrix.j;
        //    int Nz = dataMatrix.Matrix.k;
           
        //    int a;
        //    int b;
        //    int c;
        //    double a1;
        //    double a2;
        //    double a3;
        //    double a4;

        //    //initialize FA twoDMatrixLibrary.Matrix
        //    FAMatrix = new Matrix3DSingle(Nx, Ny, Nz);
        //    for (a = 0; a <= Nx - 1; a++)
        //    {
        //        for (b = 0; b <= Ny - 1; b++)
        //        {
        //            for (c = 0; c <= Nz - 1; c++)
        //            {
        //                a1 = EigenValue1.data[a, b, c] + 1E-21;
        //                //largest
        //                a2 = EigenValue2.data[a, b, c] + 1E-21;
        //                a3 = EigenValue3.data[a, b, c] + 1E-21;
        //                a4 = (a1 + a2 + a3) / 3;

        //                FAMatrix[a, b, c] = (float)(3 * ((Math.Pow((a1 - a4), 2)) + (Math.Pow((a2 - a4), 2)) + (Math.Pow((a3 - a4), 2))));
        //                FAMatrix[a, b, c] = (float)(FAMatrix[a, b, c] / (2 * ((Math.Pow(a1, 2)) + (Math.Pow(a2, 2)) + (Math.Pow(a3, 2)))));
        //                FAMatrix[a, b, c] = (float)Math.Pow(FAMatrix[a, b, c], 0.5);

        //                if (FAMatrix[a, b, c] > 1)
        //                    FAMatrix[a, b, c] = 1;
        //            }
        //        }
        //    }

        //}
        //# endregion

        //static string getCurrentCpuUsage()
        //{
        //    return cpuCounter.NextValue() + "%";
        //}

        ///*
        //Call this method every time you need to get
        //the amount of the available RAM in Mb
        //*/
        //static string getAvailableRAM()
        //{
        //    return ramCounter.NextValue().ToString() + "Mb";
        //}

    }
}